package com.gitee.wsl.mathematics.coordinate.ext

import com.gitee.wsl.mathematics.coordinate.d2.Coordinate2
import com.gitee.wsl.mathematics.coordinate.d2.MutableCoordinate2
import com.gitee.wsl.mathematics.coordinate.d3.MutableCoordinate3
import com.gitee.wsl.mathematics.vector.vec3.Vector3
import com.gitee.wsl.unit.angle.AngleUnit
import com.gitee.wsl.unit.angle.ext.cos
import com.gitee.wsl.unit.angle.ext.sin

/*infix fun <T:Number> Coordinate2<T, *>.rotatedBy(angle: T): Coordinate2<T, *> {
    val sin = sin(angle)
    val cos = cos(angle)
    return create((cos * x) - (sin * y), (sin * x) + (cos * y)) as Coordinate2<T, *>
}*/

infix fun <T:Number,V:Coordinate2<T,V>> V.rotatedBy(angle: T): V {
    val sin = sin(angle)
    val cos = cos(angle)
    return create((cos * x) - (sin * y), (sin * x) + (cos * y))
}

infix fun <T:Number> MutableCoordinate2<T, *>.rotate(angle: T){
    val sin = sin(angle)
    val cos = cos(angle)
    x = (cos * x) - (sin * y)
    y = (sin * x) + (cos * y)
    //return MutableVec2f((cos * x) - (sin * y), (sin * x) + (cos * y))
}

fun <T:Number> MutableCoordinate2<T, *>.rotate(angle: AngleUnit){
    val sin = angle.sin()
    val cos = angle.cos()
    x =( x*cos ) - ( y*sin )
    y =( x*sin ) + ( y*cos )
    //return MutableVec2f((cos * x) - (sin * y), (sin * x) + (cos * y))
}

fun <T:Number> MutableCoordinate3<T, *>.rotate(angle: AngleUnit, axisX: T, axisY: T, axisZ: T) {
    val rad = angle.radian
    val c = kotlin.math.cos(rad)
    val c1 = 1f - c
    val s = kotlin.math.sin(rad)

    val rx =   x * (axisX * axisX * c1 + c) +
                y * (axisX * axisY * c1 - axisZ * s) +
                z * (axisX * axisZ * c1 + axisY * s)

    val ry =    x * (axisY * axisX * c1 + axisZ * s) +
                y * (axisY * axisY * c1 + c) +
                z * (axisY * axisZ * c1 - axisX * s)

    val rz =    x * (axisX * axisZ * c1 - axisY * s) +
                y * (axisY * axisZ * c1 + axisX * s) +
                z * (axisZ * axisZ * c1 + c)
    x = rx
    y = ry
    z = rz
}

fun <T:Number> MutableCoordinate3<T, *>.rotate(angle: AngleUnit, axis: Vector3<T, *>) = rotate(angle, axis.x, axis.y, axis.z)